#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>

#include "app_init.h"
#include "cmsis_os2.h"
#include "osal_debug.h"
#include "soc_osal.h"
#include "errno.h"
#include "errcode.h"
#include "pinctrl.h"

#include "gpio.h"
#include "time.h"
#include "watchdog.h"
#include "watchdog_porting.h"
#include "systick.h"
#include "security_unified_porting.h"

#include "seat.h"
#include "common_config.h"
#include "watchdog.h"
#include "watchdog_porting.h"

// void set_angle_seat(unsigned int duty)
// {
//     unsigned int time = FREQ_TIME; // 假设FREQ_TIME已定义，例如20000（20ms周期）

//     uapi_gpio_set_val(SEAT_GPIO7_PIN, GPIO_LEVEL_HIGH);
//     uapi_systick_delay_us(duty);
//     uapi_gpio_set_val(SEAT_GPIO7_PIN, GPIO_LEVEL_LOW);
//     uapi_systick_delay_us(time - duty);
// }

// void start_seat_movement(int angle, int steps)
// {
//     g_seat_ctrl.target_angle = angle;
//     g_seat_ctrl.current_step = 0;
//     g_seat_ctrl.total_steps = steps;
//     g_seat_ctrl.state = SEAT_MOVING;
// }

// void update_seat_movement(void)
// {
//     if (g_seat_ctrl.state != SEAT_MOVING) return;
    
//     // 执行单步动作
//     set_angle_seat(g_seat_ctrl.target_angle);
//     g_seat_ctrl.current_step++;
    
//     // 检查是否完成
//     if (g_seat_ctrl.current_step >= g_seat_ctrl.total_steps) {
//         g_seat_ctrl.state = SEAT_COMPLETED;
//     }
// }

void SeatCtrlInit(void) {
    if(uapi_pin_set_mode(SEAT_GPIO7_PIN, SEAT_GPIO7_GPIO_MODE) != 0) {
        printf("GPIO7 set mode failed!\n");
        return;
    };
    if(uapi_gpio_set_dir(SEAT_GPIO7_PIN, GPIO_DIRECTION_OUTPUT) != 0) {
        printf("GPIO7 set dir failed!\n");
        return;
    }
}

// 座椅控制任务
// void *Seat_Task(void *arg)
// {
//     UNUSED(arg);
//     SeatCtrlInit();
    
//     int last_ctrl = -1;
//     const int MOVEMENT_STEPS = 20; // 分解为20步
    
//     while (1) {
//         // 1. 优先喂狗
//         uapi_watchdog_kick();
        
//         // 2. 获取当前座椅控制状态
//         SensorControl current = sensor_get();
//         int seat_ctrl = current.seat_ctrl;
        
//         // 3. 检查状态变化
//         if (seat_ctrl != last_ctrl || g_seat_ctrl.state == SEAT_COMPLETED) {
//             switch (seat_ctrl) {
//                 case 1: start_seat_movement(1575, MOVEMENT_STEPS); break;
//                 case 2: start_seat_movement(2500, MOVEMENT_STEPS); break;
//                 case 3: start_seat_movement(1875, MOVEMENT_STEPS); break;
//                 case 0: start_seat_movement(1575, MOVEMENT_STEPS); break;
//                 default: g_seat_ctrl.state = SEAT_IDLE; break;
//             }
//             last_ctrl = seat_ctrl;
//         }
        
//         // 4. 更新座椅状态（每次只执行一步）
//         update_seat_movement();
        
//         // 5. 让出CPU并确保喂狗
//         osDelay(1);
//         uapi_watchdog_kick();
//     }
// }
// 座椅控制状态机
typedef enum {
    SEAT_IDLE,
    SEAT_MOVING,
    SEAT_COMPLETED
} SeatState;

typedef struct {
    SeatState state;
    int target_angle;
    int current_step;
    int total_steps;
} SeatControl;

static SeatControl g_seat_ctrl = {SEAT_IDLE, 0, 0, 0};

void set_angle_seat(unsigned int duty)
{
    unsigned int time = FREQ_TIME; // 假设FREQ_TIME已定义，例如20000（20ms周期）

    uapi_gpio_set_val(SEAT_GPIO7_PIN, GPIO_LEVEL_HIGH);
    uapi_systick_delay_us(duty);
    uapi_gpio_set_val(SEAT_GPIO7_PIN, GPIO_LEVEL_LOW);
    uapi_systick_delay_us(time - duty);
}

void start_seat_movement(int angle, int steps)
{
    g_seat_ctrl.target_angle = angle;
    g_seat_ctrl.current_step = 0;
    g_seat_ctrl.total_steps = steps;
    g_seat_ctrl.state = SEAT_MOVING;
}

void update_seat_movement(void)
{
    if (g_seat_ctrl.state != SEAT_MOVING) return;
    
    // 执行单步动作
    set_angle_seat(g_seat_ctrl.target_angle);
    g_seat_ctrl.current_step++;
    
    // 检查是否完成
    if (g_seat_ctrl.current_step >= g_seat_ctrl.total_steps) {
        g_seat_ctrl.state = SEAT_COMPLETED;
    }
}

void *Seat_Task(void *arg)
{
    UNUSED(arg);
    SeatCtrlInit();
    
    int last_ctrl = -1;
    const int MOVEMENT_STEPS = 20; // 分解为20步
    while (1) {
        uapi_watchdog_kick();
        
        SensorControl current = sensor_get();
        int seat_ctrl = current.seat_ctrl;
        switch (seat_ctrl != last_ctrl?seat_ctrl:last_ctrl) {
            case 1: {
                start_seat_movement(1575, MOVEMENT_STEPS);
                while (g_seat_ctrl.state == SEAT_MOVING) {
                    update_seat_movement();
                    osDelay(20);
                    }
                break;
                }
            case 2: {
                start_seat_movement(2500, MOVEMENT_STEPS);
                while (g_seat_ctrl.state == SEAT_MOVING) {
                    update_seat_movement();
                    osDelay(20);
                    }
                break;
                }
            case 3:{
                start_seat_movement(1875, MOVEMENT_STEPS);
                while (g_seat_ctrl.state == SEAT_MOVING) {
                    update_seat_movement();
                    osDelay(20);
                    }
                break;
                }
            case 0:{
                start_seat_movement(1575, MOVEMENT_STEPS);
                while (g_seat_ctrl.state == SEAT_MOVING) {
                    update_seat_movement();
                    osDelay(20);
                    }
                break;
                }
            if(seat_ctrl != last_ctrl) last_ctrl = seat_ctrl;
        }
        // osDelay(100);
    }
    return NULL;
}

